{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "using MotionCaptureJointCalibration\n",
    "using RigidBodyDynamics\n",
    "using StaticArrays\n",
    "using ValkyrieRobot\n",
    "using ForwardDiff\n",
    "using Ipopt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a `Mechanism` (we'll use the NASA Valkyrie robot) and `MechanismState`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "MechanismState{Float64, Float64, Float64, …}(…)"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = Float64\n",
    "val = Valkyrie()\n",
    "mechanism = val.mechanism\n",
    "remove_fixed_tree_joints!(mechanism)\n",
    "state = MechanismState{T}(mechanism)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's generate a random calibration problem, as well as its ground truth solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "using MotionCaptureJointCalibration.SyntheticDataGeneration\n",
    "bodies_with_markers = findbody.(mechanism, [\"leftFoot\", \"pelvis\"])\n",
    "srand(1)\n",
    "body_weights = Dict(b => rand() for b in bodies_with_markers)\n",
    "problem, groundtruth = generate_calibration_problem(state, body_weights);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "CalibrationProblem{Float64} with:\n",
       "* 25 poses\n",
       "* 6 calibration parameters\n",
       "* 8 markers attached to 2 bodies\n",
       "\n"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "problem"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "CalibrationResult{Float64}: Optimal, residual = 0.0. Calibration parameters:\n",
       "leftHipRoll: [0.00907607]\n",
       "leftHipPitch: [-0.00809629]\n",
       "leftKneePitch: [0.000393507]\n",
       "leftAnklePitch: [-0.00972919]\n",
       "leftHipYaw: [0.00473574]\n",
       "leftAnkleRoll: [-0.00393202]"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "groundtruth"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Solve the calibration problem (without using the ground truth result):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "CalibrationResult{Float64}: Optimal, residual = 1.115199422112139e-9. Calibration parameters:\n",
       "leftHipRoll: [0.0090898]\n",
       "leftHipPitch: [-0.00804067]\n",
       "leftKneePitch: [0.000380741]\n",
       "leftAnklePitch: [-0.00980915]\n",
       "leftHipYaw: [0.00472167]\n",
       "leftAnkleRoll: [-0.00402982]"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "solver = IpoptSolver(print_level = 0, max_iter = 10000, derivative_test = \"first-order\", check_derivatives_for_naninf = \"yes\", tol = 1e-10)\n",
    "result = solve(problem, solver)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And visualize the results:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "using RigidBodyTreeInspector\n",
    "using DrakeVisualizer"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "DrakeVisualizer.any_open_windows() || (DrakeVisualizer.new_window(); sleep(1));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "MechanismState{Float64, Float64, Float64, …}(…)"
      ]
     },
     "execution_count": 59,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "vis = Visualizer()[:valkyrie]\n",
    "setgeometry!(vis, mechanism, parse_urdf(ValkyrieRobot.urdfpath(), mechanism; package_path = [ValkyrieRobot.packagepath()]))\n",
    "state = MechanismState{T}(mechanism)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 61,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/html": [],
      "text/plain": [
       "Interact.Options{:SelectionSlider,Bool}(22: \"input-11\" = false Bool , \"\", false, \"Before cal\", Interact.OptionDict(DataStructures.OrderedDict(\"Before cal\"=>false,\"After cal\"=>true), Dict(false=>\"Before cal\",true=>\"After cal\")), Any[], Any[], true, \"horizontal\")"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/html": [],
      "text/plain": [
       "Interact.Slider{Int64}(24: \"input-12\" = 1 Int64 , \"Pose number\", 1, 1:25, \"horizontal\", true, \"d\", true)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "inspect!(state, vis, problem, result)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.0",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.0"
  },
  "widgets": {
   "state": {
    "0e45b2b5-bd22-4c93-baa7-f9a848eb4533": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "1a8c9b05-530f-4275-9da6-1e01779cc299": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "488b83cc-86d5-4443-8075-a0dd0a5585ce": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "4b4e7010-b321-40f0-8d3a-a9386919e957": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "53423ecf-253f-497c-9d75-9925ecb42d2b": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "69ec2085-5dc5-47c7-aa87-9a66def54201": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "8886d8a1-aa22-4043-8eb5-42c6b8368c6e": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "ce3222a9-936d-4d2a-aa7b-d2fe370887e0": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    },
    "fcd67db0-a8b9-4de2-a784-46a06a87b7b4": {
     "views": [
      {
       "cell_index": 13
      }
     ]
    }
   },
   "version": "1.2.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
